#!/usr/bin/env python
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import cv2
import numpy as np
import sys

# 12.8
# 这份代码用来拍摄巡线所用到的图片

# 透视变换用到的参数
global src_set
global dst_set
global M_Perspective
global image_HEIGHT
global image_WID
# --------------------
# 二值化形态形态学操作用到的卷积核
global kernel_set
# --------------------
# 进行巡线检测用到的参数


# 进行参数初始化的函数
def init_lane_detect():
    # 透视变换用到的参数
    global src_set
    global dst_set
    global M_Perspective
    global image_HEIGHT
    global image_WID
    # --------------------
    # 二值化形态形态学操作用到的卷积核
    global kernel_set
    # --------------------


    # ----------------------------------------------------------------------------透视变换需要用到的参数设置
    image_HEIGHT = 480
    image_WID = 640
    # 这里获取透视变换的矩阵
    # 首先需要设定好两组四个需要标定的点
    # 第一组目标点，需要你打开原图，找到四个你需要进行变换的坐标点，你有写好测试程序路径为/调试工具/Find the coordinates of the picture.py
    src_set = np.float32([[187, 106], [426, 108], [63, 292], [555, 292]])

    # 第二组标定点就比较简单，和上面你标定的点是对应的，你这里采用了Z字型的标定顺序
    dst_set = np.float32([[80, 0], [560, 0], [80, 480], [560, 480]])
    # 这里通过调用一个API来获得变换矩阵
    M_Perspective = cv2.getPerspectiveTransform(src=src_set, dst=dst_set)

    # -------------------------------------------------------------------------------形态学操作的矩阵
    # 生成用于形态学操作的矩阵
    kernel_set = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                           ksize=(5, 5),
                                           anchor=(-1, -1)
                                           )


# 摄像头畸变矫正函数
def Straighten_the_camera(original_img):
    # 这里对摄像头的视频帧进行矫正
    # 定义相机内参矩阵，畸变矩阵，用来矫正相机畸变：相机型号HF867
    CamMax = np.array([[367.87792233, 0, 320.00047236],
                       [0, 370.36759206, 240.00000000],
                       [0, 0, 1]])

    disCef = np.array([-0.3369, 0.0870, 0, 0, 0])

    # 这里就是矫正摄像头畸变的操作
    und_st = cv2.undistort(src=original_img,
                           cameraMatrix=CamMax,
                           distCoeffs=disCef,
                           newCameraMatrix=CamMax
                           )
    return und_st


# 对赛道进行透视变换获得俯视图的函数
def warp_image(image_for_warp):
    global src_set
    global dst_set
    global M_Perspective
    global image_HEIGHT
    global image_WID

    # 下面是进行透视变换的操作
    wig_perspective_get = cv2.warpPerspective(src=image_for_warp,
                                              M=M_Perspective,
                                              dsize=(image_WID, image_HEIGHT)
                                              )

    # 返回透视变换的图片
    return wig_perspective_get


# 对图像进行二值化操作的函数(这里面包含了形态学的开操作来去除噪点)
def Binary_image(img_bina):
    global kernel_set

    Bina_image = img_bina
    # 将图像变为灰度图
    wig_Gray = cv2.cvtColor(Bina_image, cv2.COLOR_BGR2GRAY)

    # 用threshold（手动/自动设置全局阈值）对灰度图进行二值化处理
    # ret, wig_Bina_set = cv2.threshold(src=wig_Gray,
    #                                   thresh=120,
    #                                   maxval=255,
    #                                   # 这里设计这个参数，其实是让函数自己找一个全局阈值了
    #                                   type=cv2.THRESH_BINARY | cv2.THRESH_OTSU)

    ret, wig_Bina_set = cv2.threshold(src=wig_Gray,
                                      thresh=160,
                                      maxval=255,
                                      # 这里是手动设置二值化的阈值
                                      type=cv2.THRESH_BINARY)

    # 对获得的图像进行形态学的开操作，去除黑色区域的白色噪点
    open_cal_get = cv2.morphologyEx(src=wig_Bina_set,
                                    op=cv2.MORPH_OPEN,
                                    kernel=kernel_set,
                                    iterations=2)

    return open_cal_get


# 直接获得巡线需要的图像的函数
def get_lane_image(img_lane):

    warp = warp_image(img_lane)

    lane_img = Binary_image(warp)

    return lane_img




if __name__ == '__main__':
    # 获取到摄像头帧数
    cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_DSHOW)  # cv_4.2.1.30_cp38
    # cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_V4L2)  # cv_ubuntu_cp36
    # 进行参数的初始化
    init_lane_detect()
    # 不断读取摄像头图片
    while True:
        # 读取图片
        ret1, frame1 = cap_camera_wig.read()
        # 如果读取到了图片
        if ret1:
            # 进行畸变的矫正
            # camera_get = Straighten_the_camera(frame1)
            # 采用无畸变摄像头就不用进行矫正
            camera_get = frame1
        # 没有读取到就退出进程
        else:
            print("can't get the pic")
            sys.exit()

        # -------------------------------------------------
        # 进行透视操作
        warp_img = warp_image(camera_get)
        # 进行二值化操作
        bina_img = Binary_image(warp_img)


        cv2.imshow('lane_detect_pic', camera_get)
        cv2.imshow('warp_img', warp_img)
        cv2.imshow('bina_img', bina_img)



        # 等待按键指令
        key_wig = cv2.waitKey(1)
        if (key_wig & 0xFF) == ord('q'):
            print("exit")
            break
        elif (key_wig & 0xFF) == ord('s'):
            cv2.imwrite(
                # 这里的保存路径不能有英文
                "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_original.png",
                camera_get,
                None
            )
            print("the original_image has been saved")
        elif (key_wig & 0xFF) == ord('w'):
            cv2.imwrite(
                # 这里的保存路径不能有英文
                "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_warp.png",
                warp_img,
                None
            )
            print("The image of warp has been saved")

        elif (key_wig & 0xFF) == ord('b'):
            cv2.imwrite(
                # 这里的保存路径不能有英文
                "/home/nano/wig_rosfile_8_12/cv_test_ws/src/cv_test_pkg/scripts/lane_img/lane_bina.png",
                bina_img,
                None
            )
            print("The image of bina has been saved")


    # 释放视频资源
    cap_camera_wig.release()
    # 释放窗口
    cv2.destroyAllWindows()
